#include "pid.h"
#include "bsp_dwt.h"
#include "arm_math.h"

static void f_Trapezoid_Intergral(PID_t *pid);
static void f_Integral_Limit(PID_t *pid);
static void f_Derivative_On_Measurement(PID_t *pid);
static void f_Changing_Integration_Rate(PID_t *pid);
static void f_Output_Filter(PID_t *pid);
static void f_Derivative_Filter(PID_t *pid);
static void f_Output_Limit(PID_t *pid);
static void f_Proportion_Limit(PID_t *pid);
static void f_PID_ErrorHandle(PID_t *pid);

/********************************** COMMON PID **********************************/


#define LimitMax(input, max)   \
    {                          \
        if (input > max)       \
        {                      \
            input = max;       \
        }                      \
        else if (input < -max) \
        {                      \
            input = -max;      \
        }                      \
    }

/**
  * @brief          初始化 PID 控制器参数
  * @author         冉文治
  * @param          pid 指向 `pid_type_def` 结构体的指针，用于存储 PID 控制器的当前状态
  * @param          mode PID 控制器的工作模式，例如普通模式、反向模式等
  * @param          PID 一个包含三个元素的数组，表示 PID 控制器的增益系数 `[Kp, Ki, Kd]`
  * @param          max_out 最大输出限制，防止输出过大导致控制不稳定
  * @param          max_iout 最大积分输出限制，防止积分饱和
  * @retval         无
  *
  * @note           该函数会将 PID 控制器的各个参数初始化为传入值，并将相关状态变量（如误差、输出）清零。
  */
void PID_init(pid_type_def *pid, uint8_t mode, const fp32 PID[3], fp32 max_out, fp32 max_iout)
{
    if (pid == NULL || PID == NULL)
    {
        return;
    }
    pid->mode = mode;
    pid->Kp = PID[0];
    pid->Ki = PID[1];
    pid->Kd = PID[2];
    pid->max_out = max_out;
    pid->max_iout = max_iout;
    pid->Dbuf[0] = pid->Dbuf[1] = pid->Dbuf[2] = 0.0f;
    pid->error[0] = pid->error[1] = pid->error[2] = pid->Pout = pid->Iout = pid->Dout = pid->out = 0.0f;
}

/**
  * @brief          计算 PID 控制器的输出
  * @author         冉文治
  * @param          pid 指向 `pid_type_def` 结构体的指针，包含 PID 控制器的当前状态和参数
  * @param          ref 当前的反馈值，通常是传感器获取的测量值
  * @param          set 目标设定值，即期望值
  * @retval         返回计算后的 PID 输出值，用于控制量的调节
  *
  * @note           此函数根据当前 PID 控制器的工作模式（位置控制或增量控制）进行不同的计算：
  *                 - 在位置控制模式下，PID 控制器基于设定值和反馈值的差计算 PID 输出。
  *                 - 在增量控制模式下，PID 控制器基于前后误差的变化进行控制。
  *                 此外，PID 控制器的输出和积分输出会受到最大输出限制。
  */
fp32 PID_calc(pid_type_def *pid, fp32 ref, fp32 set)
{
    if (pid == NULL)
    {
        return 0.0f;
    }

    pid->error[2] = pid->error[1];
    pid->error[1] = pid->error[0];
    pid->set = set;
    pid->fdb = ref;
    pid->error[0] = set - ref;
    if (pid->mode == PID_POSITION)
    {
        pid->Pout = pid->Kp * pid->error[0];
        pid->Iout += pid->Ki * pid->error[0];
        pid->Dbuf[2] = pid->Dbuf[1];
        pid->Dbuf[1] = pid->Dbuf[0];
        pid->Dbuf[0] = (pid->error[0] - pid->error[1]);
        pid->Dout = pid->Kd * pid->Dbuf[0];
        LimitMax(pid->Iout, pid->max_iout);
        pid->out = pid->Pout + pid->Iout + pid->Dout;
        LimitMax(pid->out, pid->max_out);
    }
    else if (pid->mode == PID_DELTA)
    {
        pid->Pout = pid->Kp * (pid->error[0] - pid->error[1]);
        pid->Iout = pid->Ki * pid->error[0];
        pid->Dbuf[2] = pid->Dbuf[1];
        pid->Dbuf[1] = pid->Dbuf[0];
        pid->Dbuf[0] = (pid->error[0] - 2.0f * pid->error[1] + pid->error[2]);
        pid->Dout = pid->Kd * pid->Dbuf[0];
        pid->out += pid->Pout + pid->Iout + pid->Dout;
        LimitMax(pid->out, pid->max_out);
    }
    return pid->out;
}

/**
  * @brief          清除 PID 控制器的状态和历史数据
  * @author         冉文治
  * @param          pid 指向 `pid_type_def` 结构体的指针，包含 PID 控制器的当前状态
  * @retval         无返回值
  *
  * @note           此函数用于将 PID 控制器的所有状态变量重置为 0。主要用于重置 PID 控制器的历史误差、微分缓存和计算结果等，以便开始新的控制周期。
  *                 - `error[]` 数组：清除历史误差。
  *                 - `Dbuf[]` 数组：清除微分缓存。
  *                 - `Pout`、`Iout`、`Dout` 和 `out`：清除比例、积分、微分输出和总输出。
  *                 - `fdb` 和 `set`：清除反馈值和设定值。
  */
void PID_clear(pid_type_def *pid)
{
    if (pid == NULL)
    {
        return;
    }

    pid->error[0] = pid->error[1] = pid->error[2] = 0.0f;
    pid->Dbuf[0] = pid->Dbuf[1] = pid->Dbuf[2] = 0.0f;
    pid->out = pid->Pout = pid->Iout = pid->Dout = 0.0f;
    pid->fdb = pid->set = 0.0f;
}

/**
  * @brief          更改 PID 控制器的参数（Kp、Ki、Kd）
  * @author         冉文治
  * @param          pid 指向 `pid_type_def` 结构体的指针，包含 PID 控制器的当前状态
  * @param          Kp 目标比例增益（Proportional Gain）
  * @param          Ki 目标积分增益（Integral Gain）
  * @param          Kd 目标微分增益（Derivative Gain）
  * @retval         无返回值
  *
  * @note           此函数用于动态更改 PID 控制器的增益参数（Kp、Ki、Kd）。在某些情况下，PID 控制器的参数可能需要调整，例如：控制系统的负载变化，或为了更好地响应特定的控制目标。
  *                 修改参数后，新的增益将立即生效，但不会影响控制器的历史状态（例如误差、输出等）。
  */
void PID_change(pid_type_def *pid, fp32 Kp, fp32 Ki, fp32 Kd, fp32 max_out, fp32 max_iout)
{
    pid->Kp = Kp;
    pid->Ki = Ki;
    pid->Kd = Kd;
    pid->max_out = max_out;
    pid->max_iout = max_iout;
}

/******************************** FUZZY PID **********************************/

/**
  * @brief          定义模糊控制器中与比例增益 (Kp) 相关的模糊规则
  * @author         冉文治
  * @note           本二维数组表示模糊控制器中与比例增益 (Kp) 相关的模糊规则。
  *                 每个元素代表一个规则，其值对应于误差（e）和误差变化率（ec）组合的控制增益。
  *                 该规则阵列使用了 `PB`、`PM`、`PS`、`ZE` 等常量，这些常量代表不同的模糊值。
  */
static fp32 FuzzyRuleKpRAW[7][7] = 
{
    PB, PB, PM, PM, PS, ZE, ZE,
    PB, PB, PM, PS, PS, ZE, PS,
    PM, PM, PM, PS, ZE, PS, PS,
    PM, PM, PS, ZE, PS, PM, PM,
    PS, PS, ZE, PS, PS, PM, PM,
    PS, ZE, PS, PM, PM, PM, PB,
    ZE, ZE, PM, PM, PM, PB, PB
};

/**
  * @brief          定义模糊控制器中与积分增益 (Ki) 相关的模糊规则
  * @author         冉文治
  * @note           本二维数组表示模糊控制器中与积分增益 (Ki) 相关的模糊规则。
  *                 与比例增益规则类似，每个元素代表一个规则，对应于误差和误差变化率的组合。
  *                 此规则阵列用于计算积分增益的模糊控制值。
  */
static fp32 FuzzyRuleKiRAW[7][7] =
{
    PB, PB, PM, PM, PS, ZE, ZE,
    PB, PB, PM, PS, PS, ZE, ZE,
    PB, PM, PM, PS, ZE, PS, PS,
    PM, PM, PS, ZE, PS, PM, PM,
    PS, PS, ZE, PS, PS, PM, PB,
    ZE, ZE, PS, PS, PM, PB, PB,
    ZE, ZE, PS, PM, PM, PB, PB
};

/**
  * @brief          定义模糊控制器中与微分增益 (Kd) 相关的模糊规则
  * @author         冉文治
  * @note           本二维数组表示模糊控制器中与微分增益 (Kd) 相关的模糊规则。
  *                 该规则阵列的元素与误差和误差变化率的组合相关，用于计算微分增益的模糊控制值。
  *                 不同的组合会得到不同的微分增益（如 `PB`、`PS`、`ZE` 等）。
  */
static fp32 FuzzyRuleKdRAW[7][7] =
{
    PS, PS, PB, PB, PB, PM, PS,
    PS, PS, PB, PM, PM, PS, ZE,
    ZE, PS, PM, PM, PS, PS, ZE,
    ZE, PS, PS, PS, PS, PS, ZE,
    ZE, ZE, ZE, ZE, ZE, ZE, ZE,
    PB, PS, PS, PS, PS, PS, PB,
    PB, PM, PM, PM, PS, PS, PB
};

/**
  * @brief          初始化模糊控制规则参数
  * @author         冉文治
  * @param          fuzzyRule 指向 `FuzzyRule_t` 结构体的指针，包含模糊控制器的相关参数和规则
  * @param          fuzzyRuleKp 指向一个 `7x7` 大小的二维数组，表示模糊控制器中与比例增益（Kp）相关的模糊规则（可以传入自定义规则数组，若传入 NULL，则使用默认规则）
  * @param          fuzzyRuleKi 指向一个 `7x7` 大小的二维数组，表示模糊控制器中与积分增益（Ki）相关的模糊规则（可以传入自定义规则数组，若传入 NULL，则使用默认规则）
  * @param          fuzzyRuleKd 指向一个 `7x7` 大小的二维数组，表示模糊控制器中与微分增益（Kd）相关的模糊规则（可以传入自定义规则数组，若传入 NULL，则使用默认规则）
  * @param          kpRatio 比例增益的模糊调整系数（范围为0到1）
  * @param          kiRatio 积分增益的模糊调整系数（范围为0到1）
  * @param          kdRatio 微分增益的模糊调整系数（范围为0到1）
  * @param          eStep 每步误差的增量，决定了误差的分辨率（误差步长）
  * @param          ecStep 每步误差变化量的增量，决定了误差变化的分辨率（误差变化步长）
  * @retval         无返回值
  *
  * @note           此函数用于初始化模糊控制规则及相关参数。它允许用户传入自定义的模糊规则（Kp、Ki、Kd），若未传入，则使用默认的规则阵列。
  *                 同时，用户可以调整模糊控制器的增益比例系数（`kpRatio`、`kiRatio`、`kdRatio`）和误差步长（`eStep` 和 `ecStep`），
  *                 这些参数控制模糊规则的精度和控制响应速度。若 `eStep` 或 `ecStep` 设置过小，则默认值为 1。
  */
void Fuzzy_Rule_Init(FuzzyRule_t *fuzzyRule, fp32 (*fuzzyRuleKp)[7], fp32 (*fuzzyRuleKi)[7], fp32 (*fuzzyRuleKd)[7], fp32 kpRatio, fp32 kiRatio, fp32 kdRatio, fp32 eStep, fp32 ecStep)
{
    // 如果没有传入模糊规则数组，使用默认的规则阵列
    if (fuzzyRuleKp == NULL)
        fuzzyRule->FuzzyRuleKp = FuzzyRuleKpRAW;
    else
        fuzzyRule->FuzzyRuleKp = fuzzyRuleKp;
    if (fuzzyRuleKi == NULL)
        fuzzyRule->FuzzyRuleKi = FuzzyRuleKiRAW;
    else
        fuzzyRule->FuzzyRuleKi = fuzzyRuleKi;
    if (fuzzyRuleKd == NULL)
        fuzzyRule->FuzzyRuleKd = FuzzyRuleKdRAW;
    else
        fuzzyRule->FuzzyRuleKd = fuzzyRuleKd;

    // 设置增益系数
    fuzzyRule->KpRatio = kpRatio;
    fuzzyRule->KiRatio = kiRatio;
    fuzzyRule->KdRatio = kdRatio;

    // 设置误差步长，如果步长过小，使用默认值1
    if (eStep < 0.00001f)
        eStep = 1;
    if (ecStep < 0.00001f)
        ecStep = 1;
    fuzzyRule->eStep = eStep;
    fuzzyRule->ecStep = ecStep;
}

/**
  * @brief          实现模糊控制器的规则计算，基于当前的误差（e）和误差变化率（ec）计算比例（Kp）、积分（Ki）和微分（Kd）增益。
  * @author         冉文治
  * @param          fuzzyRule  指向 `FuzzyRule_t` 结构体的指针，包含模糊控制器的当前状态和参数
  * @param          measure    当前测量值（反馈量）
  * @param          ref        设定值（目标值）
  * @retval         无返回值。结果保存在 `fuzzyRule` 结构体中，更新了 `KpFuzzy`、`KiFuzzy` 和 `KdFuzzy`。
  *
  * @note           该函数根据当前的误差（e）和误差变化率（ec），通过模糊规则阵列计算相应的模糊控制增益 Kp、Ki 和 Kd。
  *                 模糊推理使用了误差和误差变化率的当前值及其在模糊规则中的索引，利用模糊控制增益的加权平均进行计算。
  *                 最终的模糊控制增益将用于调整 PID 控制器的输出。
  */
void Fuzzy_Rule_Implementation(FuzzyRule_t *fuzzyRule, fp32 measure, fp32 ref)
{
    fp32 eLeftTemp, ecLeftTemp;
    fp32 eRightTemp, ecRightTemp;
    int32_t eLeftIndex, ecLeftIndex;
    int32_t eRightIndex, ecRightIndex;

    // 计算时间间隔
    fuzzyRule->dt = DWT_GetDeltaT((void *)fuzzyRule->DWT_CNT);

    // 计算误差（e）和误差变化率（ec）
    fuzzyRule->e = ref - measure;
    fuzzyRule->ec = (fuzzyRule->e - fuzzyRule->eLast) / fuzzyRule->dt;
    fuzzyRule->eLast = fuzzyRule->e;

    // 根据误差（e）和误差变化率（ec）确定模糊规则的索引
    eLeftIndex = fuzzyRule->e >= 3 * fuzzyRule->eStep ? 6 : (fuzzyRule->e <= -3 * fuzzyRule->eStep ? 0 : (fuzzyRule->e >= 0 ? ((int32_t)(fuzzyRule->e / fuzzyRule->eStep) + 3) : ((int)(fuzzyRule->e / fuzzyRule->eStep) + 2)));
    eRightIndex = fuzzyRule->e >= 3 * fuzzyRule->eStep ? 6 : (fuzzyRule->e <= -3 * fuzzyRule->eStep ? 0 : (fuzzyRule->e >= 0 ? ((int32_t)(fuzzyRule->e / fuzzyRule->eStep) + 4) : ((int)(fuzzyRule->e / fuzzyRule->eStep) + 3)));
    ecLeftIndex = fuzzyRule->ec >= 3 * fuzzyRule->ecStep ? 6 : (fuzzyRule->ec <= -3 * fuzzyRule->ecStep ? 0 : (fuzzyRule->ec >= 0 ? ((int32_t)(fuzzyRule->ec / fuzzyRule->ecStep) + 3) : ((int)(fuzzyRule->ec / fuzzyRule->ecStep) + 2)));
    ecRightIndex = fuzzyRule->ec >= 3 * fuzzyRule->ecStep ? 6 : (fuzzyRule->ec <= -3 * fuzzyRule->ecStep ? 0 : (fuzzyRule->ec >= 0 ? ((int32_t)(fuzzyRule->ec / fuzzyRule->ecStep) + 4) : ((int)(fuzzyRule->ec / fuzzyRule->ecStep) + 3)));

    eLeftTemp = fuzzyRule->e >= 3 * fuzzyRule->eStep ? 0 : (fuzzyRule->e <= -3 * fuzzyRule->eStep ? 1 : (eRightIndex - fuzzyRule->e / fuzzyRule->eStep - 3));
    eRightTemp = fuzzyRule->e >= 3 * fuzzyRule->eStep ? 1 : (fuzzyRule->e <= -3 * fuzzyRule->eStep ? 0 : (fuzzyRule->e / fuzzyRule->eStep - eLeftIndex + 3));
    ecLeftTemp = fuzzyRule->ec >= 3 * fuzzyRule->ecStep ? 0 : (fuzzyRule->ec <= -3 * fuzzyRule->ecStep ? 1 : (ecRightIndex - fuzzyRule->ec / fuzzyRule->ecStep - 3));
    ecRightTemp = fuzzyRule->ec >= 3 * fuzzyRule->ecStep ? 1 : (fuzzyRule->ec <= -3 * fuzzyRule->ecStep ? 0 : (fuzzyRule->ec / fuzzyRule->ecStep - ecLeftIndex + 3));

    // 根据模糊规则阵列计算加权平均的模糊增益（Kp、Ki 和 Kd）
    fuzzyRule->KpFuzzy = eLeftTemp * ecLeftTemp * fuzzyRule->FuzzyRuleKp[eLeftIndex][ecLeftIndex] + eLeftTemp * ecRightTemp * fuzzyRule->FuzzyRuleKp[eRightIndex][ecLeftIndex] + eRightTemp * ecLeftTemp * fuzzyRule->FuzzyRuleKp[eLeftIndex][ecRightIndex] + eRightTemp * ecRightTemp * fuzzyRule->FuzzyRuleKp[eRightIndex][ecRightIndex];
    fuzzyRule->KiFuzzy = eLeftTemp * ecLeftTemp * fuzzyRule->FuzzyRuleKi[eLeftIndex][ecLeftIndex] + eLeftTemp * ecRightTemp * fuzzyRule->FuzzyRuleKi[eRightIndex][ecLeftIndex] + eRightTemp * ecLeftTemp * fuzzyRule->FuzzyRuleKi[eLeftIndex][ecRightIndex] + eRightTemp * ecRightTemp * fuzzyRule->FuzzyRuleKi[eRightIndex][ecRightIndex];
    fuzzyRule->KdFuzzy = eLeftTemp * ecLeftTemp * fuzzyRule->FuzzyRuleKd[eLeftIndex][ecLeftIndex] + eLeftTemp * ecRightTemp * fuzzyRule->FuzzyRuleKd[eRightIndex][ecLeftIndex] + eRightTemp * ecLeftTemp * fuzzyRule->FuzzyRuleKd[eLeftIndex][ecRightIndex] + eRightTemp * ecRightTemp * fuzzyRule->FuzzyRuleKd[eRightIndex][ecRightIndex];
}

/******************************* PID CONTROL *********************************/

/**
  * @brief          初始化 PID 控制器的参数
  * @author         冉文治
  * @param          pid               指向 PID 结构体的指针，用于存储初始化后的 PID 控制器状态
  * @param          max_out           PID 控制器输出的最大值
  * @param          intergral_limit   积分限制值，用于防止积分饱和
  * @param          deadband          死区值，用于忽略小范围的误差
  * @param          kp                比例增益
  * @param          Ki                积分增益
  * @param          Kd                微分增益
  * @param          A                 微分增益滤波器的系数 A
  * @param          B                 微分增益滤波器的系数 B
  * @param          output_lpf_rc     输出信号的低通滤波器时间常数
  * @param          derivative_lpf_rc 微分信号的低通滤波器时间常数
  * @param          ols_order         普通最小二乘（OLS）算法的阶数，用于微分信号的平滑
  * @param          improve           增益调节优化标志
  * @retval         无返回值。初始化后的 PID 控制器将保存在 `pid` 结构体中。
  *
  * @note           此函数用于初始化 PID 控制器的各种参数，包括比例、积分、微分增益（Kp、Ki、Kd），以及一些附加功能（如积分限制、死区值、低通滤波器、微分滤波等）。
  *                 此外，该函数还对 PID 控制器内部的普通最小二乘（OLS）算法进行初始化，并为 PID 控制器的错误处理和输出进行设置。
  */
void PID_Init(PID_t *pid, fp32 max_out, fp32 intergral_limit, fp32 deadband, fp32 kp, fp32 Ki, fp32 Kd, fp32 A, fp32 B, fp32 output_lpf_rc, fp32 derivative_lpf_rc, uint16_t ols_order, uint8_t improve)
{
    // 设置 PID 控制器的最大输出、积分限制、死区值
    pid->DeadBand = deadband;
    pid->IntegralLimit = intergral_limit;
    pid->MaxOut = max_out;
    pid->Ref = 0;

    // 设置 PID 控制器的增益参数
    pid->Kp = kp;
    pid->Ki = Ki;
    pid->Kd = Kd;
    pid->ITerm = 0;

    // 设置微分滤波器的系数
    pid->CoefA = A;
    pid->CoefB = B;

    // 设置输出信号的低通滤波器时间常数
    pid->Output_LPF_RC = output_lpf_rc;

    // 设置微分信号的低通滤波器时间常数
    pid->Derivative_LPF_RC = derivative_lpf_rc;

    // 设置普通最小二乘（OLS）算法的阶数并初始化
    pid->OLS_Order = ols_order;
    OLS_Init(&pid->OLS, ols_order);

    // 初始化 DWT 计数器
    pid->DWT_CNT = 0;

    // 设置增益调节优化标志
    pid->Improve = improve;

    // 初始化 PID 错误处理器
    pid->ERRORHandler.ERRORCount = 0;
    pid->ERRORHandler.ERRORType = PID_ERROR_NONE;

    // 初始化输出
    pid->Output = 0;
}

/**
  * @brief          计算 PID 控制器的输出
  * @author         冉文治
  * @param          pid                指向 PID 结构体的指针，包含 PID 控制器的状态和参数
  * @param          measure            当前测量值（反馈值）
  * @param          ref                参考值（目标值）
  * @retval         返回 PID 控制器的输出值
  *
  * @note           此函数根据当前的测量值和参考值，计算 PID 控制器的输出。控制器计算过程包括以下步骤：
  *                 - 计算误差（参考值与测量值之差）
  *                 - 根据误差、增益（Kp、Ki、Kd）、时间间隔等计算 PID 控制器的各项输出（P、I、D）
  *                 - 支持模糊控制和误差处理，包含增益优化、积分限幅、微分滤波等功能
  *                 - 支持积分限幅、积分率调整、微分计算等功能，确保控制器的稳定性和精度
  *                 - 最终输出根据 PID 控制器的参数、改善标志和外部回调函数进行调整
  */
fp32 PID_Calculate(PID_t *pid, fp32 measure, fp32 ref)
{
    // 错误处理
    if (pid->Improve & ErrorHandle)
        f_PID_ErrorHandle(pid);

    // 计算时间间隔
    pid->dt = DWT_GetDeltaT((void *)&pid->DWT_CNT);

    // 存储当前测量值和参考值
    pid->Measure = measure;
    pid->Ref = ref;

    // 计算当前误差
    pid->Err = pid->Ref - pid->Measure;

    // 调用用户自定义的函数（如果有）
    if (pid->User_Func1_f != NULL)
        pid->User_Func1_f(pid);

    // 如果误差大于死区值，进行 PID 计算
    if (abs(pid->Err) > pid->DeadBand)
    {
        // 如果没有启用模糊控制
        if (pid->FuzzyRule == NULL)
        {
            pid->Pout = pid->Kp * pid->Err;
            pid->ITerm = pid->Ki * pid->Err * pid->dt;
            // 根据 OLS（普通最小二乘）算法计算微分项
            if (pid->OLS_Order > 2)
                pid->Dout = pid->Kd * OLS_Derivative(&pid->OLS, pid->dt, pid->Err);
            else
                pid->Dout = pid->Kd * (pid->Err - pid->Last_Err) / pid->dt;
        }
        // 启用模糊控制时，加入模糊增益
        else
        {
            pid->Pout = (pid->Kp + pid->FuzzyRule->KpFuzzy) * pid->Err;
            pid->ITerm = (pid->Ki + pid->FuzzyRule->KiFuzzy) * pid->Err * pid->dt;
            if (pid->OLS_Order > 2)
                pid->Dout = (pid->Kd + pid->FuzzyRule->KdFuzzy) * OLS_Derivative(&pid->OLS, pid->dt, pid->Err);
            else
                pid->Dout = (pid->Kd + pid->FuzzyRule->KdFuzzy) * (pid->Err - pid->Last_Err) / pid->dt;
        }

        // 调用用户自定义的函数（如果有）
        if (pid->User_Func2_f != NULL)
            pid->User_Func2_f(pid);

        // 根据改善标志选择是否进行额外的优化处理
        if (pid->Improve & Trapezoid_Intergral)
            f_Trapezoid_Intergral(pid); // 梯形积分法
        if (pid->Improve & ChangingIntegrationRate)
            f_Changing_Integration_Rate(pid); // 变化积分率
        if (pid->Improve & Derivative_On_Measurement)
            f_Derivative_On_Measurement(pid); // 误差基于测量计算微分
        if (pid->Improve & DerivativeFilter)
            f_Derivative_Filter(pid); // 微分信号滤波
        if (pid->Improve & Integral_Limit)
            f_Integral_Limit(pid); // 积分限制

        // 更新积分项
        pid->Iout += pid->ITerm;

        // 计算最终输出
        pid->Output = pid->Pout + pid->Iout + pid->Dout;

        // 根据改善标志进行输出滤波
        if (pid->Improve & OutputFilter)
            f_Output_Filter(pid); // 输出滤波

        // 限制输出值
        f_Output_Limit(pid);

        // 限制比例项
        f_Proportion_Limit(pid);
    }

    // 存储当前的测量值、输出、微分项、误差等，用于下一次计算
    pid->Last_Measure = pid->Measure;
    pid->Last_Output = pid->Output;
    pid->Last_Dout = pid->Dout;
    pid->Last_Err = pid->Err;
    pid->Last_ITerm = pid->ITerm;

    // 返回 PID 控制器的输出值
    return pid->Output;
}

/**
  * @brief          梯形积分法
  * @author         冉文治
  * @param          pid               指向 PID 结构体的指针，包含 PID 控制器的状态和参数
  * @retval         无返回值。根据梯形积分法更新 PID 控制器的积分项。
  *
  * @note           此函数实现了梯形积分法，用于平滑积分过程。通过计算当前误差和上一时刻误差的平均值，再乘以积分增益和时间间隔，来更新 PID 控制器的积分项（`ITerm`）。
  *                 如果启用了模糊控制，积分增益会加上模糊控制计算得到的增益。
  */
static void f_Trapezoid_Intergral(PID_t *pid)
{
    // 如果没有启用模糊控制
    if (pid->FuzzyRule == NULL)
        pid->ITerm = pid->Ki * ((pid->Err + pid->Last_Err) / 2) * pid->dt;
    // 如果启用了模糊控制
    else
        pid->ITerm = (pid->Ki + pid->FuzzyRule->KiFuzzy) * ((pid->Err + pid->Last_Err) / 2) * pid->dt;
}

/**
  * @brief          动态调整积分率
  * @author         冉文治
  * @param          pid               指向 PID 结构体的指针，包含 PID 控制器的状态和参数
  * @retval         无返回值。根据当前误差和输出，动态调整 PID 控制器的积分项。
  *
  * @note           此函数根据当前误差和控制器的积分输出值（`Iout`）动态调整积分项（`ITerm`）。如果误差和积分项同符号，则根据误差的大小动态调整积分增益。
  *                 通过调整系数 A 和 B 的值，可以控制积分项的变化速率，以避免积分过大导致控制器失控。
  */
static void f_Changing_Integration_Rate(PID_t *pid)
{
    // 如果误差和积分输出同符号
    if (pid->Err * pid->Iout > 0)
    {
        // 如果误差小于等于 CoefB，则不改变积分项
        if (abs(pid->Err) <= pid->CoefB)
            return;

        // 如果误差在 CoefA + CoefB 范围内，按比例调整积分项
        if (abs(pid->Err) <= (pid->CoefA + pid->CoefB))
            pid->ITerm *= (pid->CoefA - abs(pid->Err) + pid->CoefB) / pid->CoefA;
        // 如果误差超过此范围，则将积分项清零
        else
            pid->ITerm = 0;
    }
}

/**
  * @brief          限制积分项的值
  * @author         冉文治
  * @param          pid               指向 PID 结构体的指针，包含 PID 控制器的状态和参数
  * @retval         无返回值。限制 PID 控制器的积分项（`ITerm` 和 `Iout`）以避免积分饱和。
  *
  * @note           此函数用于限制 PID 控制器的积分项。在计算过程中，如果积分输出和误差同符号且输出超出最大输出限值，则会将积分项清零。
  *                 此外，函数还会限制积分项的最大值和最小值，以避免积分过大导致控制器失控。积分项超出限制时，会直接设置为限制值。
  */
static void f_Integral_Limit(PID_t *pid)
{
    static fp32 temp_Output, temp_Iout;

    // 临时计算积分输出和总输出
    temp_Iout = pid->Iout + pid->ITerm;
    temp_Output = pid->Pout + pid->Iout + pid->Dout;

    // 如果总输出超出最大输出限制
    if (abs(temp_Output) > pid->MaxOut)
    {
        // 如果误差和积分输出同符号，清零积分项
        if (pid->Err * pid->Iout > 0)
        {
            pid->ITerm = 0;
        }
    }

    // 如果积分输出超出限制，设置积分项为零，并限制积分输出在最大值范围内
    if (temp_Iout > pid->IntegralLimit)
    {
        pid->ITerm = 0;
        pid->Iout = pid->IntegralLimit;
    }
    // 如果积分输出低于负限制，设置积分项为零，并限制积分输出在最小值范围内
    if (temp_Iout < -pid->IntegralLimit)
    {
        pid->ITerm = 0;
        pid->Iout = -pid->IntegralLimit;
    }
}

/**
  * @brief          基于测量值计算微分项
  * @author         冉文治
  * @param          pid               指向 PID 结构体的指针，包含 PID 控制器的状态和参数
  * @retval         无返回值。根据当前的测量值和 PID 控制器的配置计算微分项。
  *
  * @note           此函数根据 PID 控制器的配置计算微分项（`Dout`）。如果启用了普通最小二乘（OLS）算法，则通过 OLS 算法进行微分计算；否则，使用标准的微分公式。
  *                 如果启用了模糊控制规则，则微分增益会与模糊规则的 `KdFuzzy` 进行组合。
  *                 此函数的核心目的是计算测量值对 PID 控制器的微分项影响，并返回最终的微分输出。
  */
static void f_Derivative_On_Measurement(PID_t *pid)
{
    if (pid->FuzzyRule == NULL)
    {
        // 如果启用了 OLS，使用 OLS 进行微分计算
        if (pid->OLS_Order > 2)
            pid->Dout = pid->Kd * OLS_Derivative(&pid->OLS, pid->dt, -pid->Measure);
        else
            pid->Dout = pid->Kd * (pid->Last_Measure - pid->Measure) / pid->dt;
    }
    else
    {
        // 如果启用了 OLS，使用 OLS 进行微分计算，并结合模糊控制的微分增益
        if (pid->OLS_Order > 2)
            pid->Dout = (pid->Kd + pid->FuzzyRule->KdFuzzy) * OLS_Derivative(&pid->OLS, pid->dt, -pid->Measure);
        else
            pid->Dout = (pid->Kd + pid->FuzzyRule->KdFuzzy) * (pid->Last_Measure - pid->Measure) / pid->dt;
    }
}

/**
  * @brief          对微分项应用低通滤波器
  * @author         冉文治
  * @param          pid               指向 PID 结构体的指针，包含 PID 控制器的状态和参数
  * @retval         无返回值。更新 `Dout`，使其经过低通滤波处理。
  *
  * @note           该函数通过低通滤波器对计算出的微分项进行滤波。滤波器的时间常数由 `Derivative_LPF_RC` 参数控制。
  *                 低通滤波器的作用是平滑微分信号，减少高频噪声的影响，从而提升系统的稳定性。
  */
static void f_Derivative_Filter(PID_t *pid)
{
    pid->Dout = pid->Dout * pid->dt / (pid->Derivative_LPF_RC + pid->dt) + pid->Last_Dout * pid->Derivative_LPF_RC / (pid->Derivative_LPF_RC + pid->dt);
}

/**
  * @brief          对 PID 输出应用低通滤波器
  * @author         冉文治
  * @param          pid               指向 PID 结构体的指针，包含 PID 控制器的状态和参数
  * @retval         无返回值。更新 `Output`，使其经过低通滤波处理。
  *
  * @note           该函数通过低通滤波器对 PID 控制器的输出进行滤波。滤波器的时间常数由 `Output_LPF_RC` 参数控制。
  *                 该滤波器主要用于减少输出信号的噪声，保证系统输出的平稳性。
  */
static void f_Output_Filter(PID_t *pid)
{
    pid->Output = pid->Output * pid->dt / (pid->Output_LPF_RC + pid->dt) + pid->Last_Output * pid->Output_LPF_RC / (pid->Output_LPF_RC + pid->dt);
}

/**
  * @brief          限制 PID 控制器的输出值
  * @author         冉文治
  * @param          pid               指向 PID 结构体的指针，包含 PID 控制器的状态和参数
  * @retval         无返回值。根据最大输出限制 `MaxOut`，更新 PID 输出。
  *
  * @note           该函数根据 `MaxOut` 参数对输出进行限制。如果输出值超过 `MaxOut`，则将其限制在最大值或最小值范围内，防止输出过大。
  */
static void f_Output_Limit(PID_t *pid)
{
    if (pid->Output > pid->MaxOut)
    {
        pid->Output = pid->MaxOut;
    }
    if (pid->Output < -(pid->MaxOut))
    {
        pid->Output = -(pid->MaxOut);
    }
}

/**
  * @brief          限制 PID 控制器的比例项（Pout）
  * @author         冉文治
  * @param          pid               指向 PID 结构体的指针，包含 PID 控制器的状态和参数
  * @retval         无返回值。根据最大输出限制 `MaxOut`，更新比例项。
  *
  * @note           该函数根据 `MaxOut` 参数对比例项进行限制。如果比例项 `Pout` 超过 `MaxOut`，则将其限制在最大值或最小值范围内。
  *                 这样可以避免比例项过大导致控制系统不稳定。
  */
static void f_Proportion_Limit(PID_t *pid)
{
    if (pid->Pout > pid->MaxOut)
    {
        pid->Pout = pid->MaxOut;
    }
    if (pid->Pout < -(pid->MaxOut))
    {
        pid->Pout = -(pid->MaxOut);
    }
}

/**
  * @brief          处理 PID 控制器的错误监测
  * @author         冉文治
  * @param          pid               指向 PID 结构体的指针，包含 PID 控制器的状态和参数
  * @retval         无返回值。根据错误条件更新 `ERRORHandler` 结构中的错误计数和错误类型。
  *
  * @note           该函数用于检测 PID 控制器的输出是否符合预期，并在输出异常时进行错误处理。
  *                 如果控制输出小于 0.1% 的最大输出值或参考值小于 0.0001，则认为没有发生错误。
  *                 如果误差相对于参考值超过 95%，则增加错误计数器。若错误计数器超过 500次，视为电机堵转（`Motor_Blocked`）。
  */
static void f_PID_ErrorHandle(PID_t *pid)
{
    if (pid->Output < pid->MaxOut * 0.001f || fabsf(pid->Ref) < 0.0001f)
        return;

    if ((fabsf(pid->Ref - pid->Measure) / fabsf(pid->Ref)) > 0.95f)
    {
        pid->ERRORHandler.ERRORCount++;
    }
    else
    {
        pid->ERRORHandler.ERRORCount = 0;
    }

    if (pid->ERRORHandler.ERRORCount > 500)
    {
        // Motor blocked over 1000times
        pid->ERRORHandler.ERRORType = Motor_Blocked;
    }
}

/*************************** FEEDFORWARD CONTROL *****************************/

/**
  * @brief          初始化前馈控制器的参数
  * @author         冉文治
  * @param          ffc               指向前馈控制器结构体的指针，用于存储初始化后的控制器状态
  * @param          max_out           前馈控制器输出的最大值
  * @param          c                 前馈控制器的系数数组（c[0], c[1], c[2]），用于计算前馈输出
  * @param          lpf_rc            前馈信号的低通滤波器时间常数
  * @param          ref_dot_ols_order 参考信号一阶导数（速度）的普通最小二乘（OLS）算法阶数
  * @param          ref_ddot_ols_order 参考信号二阶导数（加速度）的普通最小二乘（OLS）算法阶数
  * @retval         无返回值。初始化后的前馈控制器将保存在 `ffc` 结构体中。
  *
  * @note           此函数用于初始化前馈控制器的各项参数，包括系数、低通滤波器时间常数、最小二乘法阶数等。
  *                 如果提供的系数数组 `c` 为 NULL，则使用默认值 `0` 初始化系数。
  *                 此外，还初始化了参考信号一阶导数和二阶导数的普通最小二乘法（OLS）算法。
  */
void Feedforward_Init(Feedforward_t *ffc, fp32 max_out, fp32 *c, fp32 lpf_rc, uint16_t ref_dot_ols_order, uint16_t ref_ddot_ols_order)
{
    // 设置输出的最大值
    ffc->MaxOut = max_out;

    // 初始化系数数组 c
    if (c != NULL && ffc != NULL)
    {
        ffc->c[0] = c[0];
        ffc->c[1] = c[1];
        ffc->c[2] = c[2];
    }
    else
    {
        ffc->c[0] = 0;
        ffc->c[1] = 0;
        ffc->c[2] = 0;
        ffc->MaxOut = 0;
    }

    // 设置低通滤波器时间常数
    ffc->LPF_RC = lpf_rc;

    // 设置参考信号一阶和二阶导数的 OLS 阶数
    ffc->Ref_dot_OLS_Order = ref_dot_ols_order;
    ffc->Ref_ddot_OLS_Order = ref_ddot_ols_order;

    // 如果 OLS 阶数大于 2，则初始化 OLS 相关结构
    if (ref_dot_ols_order > 2)
        OLS_Init(&ffc->Ref_dot_OLS, ref_dot_ols_order);
    if (ref_ddot_ols_order > 2)
        OLS_Init(&ffc->Ref_ddot_OLS, ref_ddot_ols_order);

    // 初始化计时器和输出
    ffc->DWT_CNT = 0;
    ffc->Output = 0;
}

/**
  * @brief          计算前馈控制器的输出
  * @author         冉文治
  * @param          ffc               指向前馈控制器结构体的指针，包含当前的控制器状态和参数
  * @param          ref               参考信号
  * @retval         前馈控制器的输出值
  *
  * @note           此函数根据参考信号及其导数（速度、加速度）计算前馈控制器的输出值。
  *                 参考信号的值会经过低通滤波处理，然后计算其一阶和二阶导数（速度和加速度）。
  *                 最后，输出值根据预设的系数进行加权求和，并限制在最大输出范围内。
  */
fp32 Feedforward_Calculate(Feedforward_t *ffc, fp32 ref)
{
    // 获取时间增量
    ffc->dt = DWT_GetDeltaT((void *)&ffc->DWT_CNT);

    // 对参考信号进行低通滤波处理
    ffc->Ref = ref * ffc->dt / (ffc->LPF_RC + ffc->dt) + ffc->Ref * ffc->LPF_RC / (ffc->LPF_RC + ffc->dt);

    // 计算参考信号的一阶导数（速度）
    if (ffc->Ref_dot_OLS_Order > 2)
        ffc->Ref_dot = OLS_Derivative(&ffc->Ref_dot_OLS, ffc->dt, ffc->Ref);
    else
        ffc->Ref_dot = (ffc->Ref - ffc->Last_Ref) / ffc->dt;

    // 计算参考信号的二阶导数（加速度）
    if (ffc->Ref_ddot_OLS_Order > 2)
        ffc->Ref_ddot = OLS_Derivative(&ffc->Ref_ddot_OLS, ffc->dt, ffc->Ref_dot);
    else
        ffc->Ref_ddot = (ffc->Ref_dot - ffc->Last_Ref_dot) / ffc->dt;

    // 根据系数计算前馈输出
    ffc->Output = ffc->c[0] * ffc->Ref + ffc->c[1] * ffc->Ref_dot + ffc->c[2] * ffc->Ref_ddot;

    // 限制输出在最大输出范围内
    ffc->Output = fp32_constrain(ffc->Output, -ffc->MaxOut, ffc->MaxOut);

    // 保存当前参考值和参考速度，用于下一次计算
    ffc->Last_Ref = ffc->Ref;
    ffc->Last_Ref_dot = ffc->Ref_dot;

    return ffc->Output;
}

/*************************LINEAR DISTURBANCE OBSERVER *************************/

/**
  * @brief          初始化 LDOB (Luenberger Disturbance Observer) 控制器的参数
  * @author         冉文治
  * @param          ldob               指向 LDOB 控制器结构体的指针，用于存储初始化后的控制器状态
  * @param          max_d              最大干扰值，用于限制干扰估计的输出范围
  * @param          deadband           死区值，用于忽略小范围的干扰
  * @param          c                  LDOB 控制器的系数数组（c[0], c[1], c[2]），用于计算干扰估计
  * @param          lpf_rc             低通滤波器的时间常数，用于平滑干扰估计
  * @param          measure_dot_ols_order  测量信号一阶导数（速度）的普通最小二乘（OLS）算法阶数
  * @param          measure_ddot_ols_order 测量信号二阶导数（加速度）的普通最小二乘（OLS）算法阶数
  * @retval         无返回值。初始化后的 LDOB 控制器将保存在 `ldob` 结构体中。
  *
  * @note           此函数用于初始化 LDOB 控制器的各项参数，包括系数、低通滤波器常数、最小二乘法阶数等。
  *                 如果提供的系数数组 `c` 为 NULL，则使用默认值 `0` 初始化系数。
  *                 此外，还初始化了测量信号一阶导数和二阶导数的普通最小二乘法（OLS）算法。
  */
void LDOB_Init(LDOB_t *ldob, fp32 max_d, fp32 deadband, fp32 *c, fp32 lpf_rc, uint16_t measure_dot_ols_order, uint16_t measure_ddot_ols_order)
{
    // 设置最大干扰值
    ldob->Max_Disturbance = max_d;

    // 设置死区值
    ldob->DeadBand = deadband;

    // 初始化系数数组 c
    if (c != NULL && ldob != NULL)
    {
        ldob->c[0] = c[0];
        ldob->c[1] = c[1];
        ldob->c[2] = c[2];
    }
    else
    {
        ldob->c[0] = 0;
        ldob->c[1] = 0;
        ldob->c[2] = 0;
        ldob->Max_Disturbance = 0;
    }

    // 设置低通滤波器时间常数
    ldob->LPF_RC = lpf_rc;

    // 设置测量信号一阶和二阶导数的 OLS 阶数
    ldob->Measure_dot_OLS_Order = measure_dot_ols_order;
    ldob->Measure_ddot_OLS_Order = measure_ddot_ols_order;

    // 如果 OLS 阶数大于 2，则初始化 OLS 相关结构
    if (measure_dot_ols_order > 2)
        OLS_Init(&ldob->Measure_dot_OLS, measure_dot_ols_order);
    if (measure_ddot_ols_order > 2)
        OLS_Init(&ldob->Measure_ddot_OLS, measure_ddot_ols_order);

    // 初始化计时器和输出
    ldob->DWT_CNT = 0;
    ldob->Disturbance = 0;
}

/**
  * @brief          计算 LDOB (Luenberger Disturbance Observer) 控制器的输出
  * @author         冉文治
  * @param          ldob               指向 LDOB 控制器结构体的指针，包含当前的控制器状态和参数
  * @param          measure            测量信号
  * @param          u                  控制输入
  * @retval         控制器的输出值（估计的干扰）
  *
  * @note           此函数根据测量信号及其导数（速度、加速度）估算系统的干扰并输出。
  *                 测量信号会经过低通滤波器进行平滑，并计算其一阶和二阶导数（速度和加速度）。
  *                 然后根据系数计算干扰估计值，输出会受到最大干扰值的限制。
  *                 如果估计的干扰大于死区范围，则输出该干扰值，否则输出 0。
  */
fp32 LDOB_Calculate(LDOB_t *ldob, fp32 measure, fp32 u)
{
    // 获取时间增量
    ldob->dt = DWT_GetDeltaT((void *)&ldob->DWT_CNT);

    // 设置测量信号和控制输入
    ldob->Measure = measure;
    ldob->u = u;

    // 计算测量信号的一阶导数（速度）
    if (ldob->Measure_dot_OLS_Order > 2)
        ldob->Measure_dot = OLS_Derivative(&ldob->Measure_dot_OLS, ldob->dt, ldob->Measure);
    else
        ldob->Measure_dot = (ldob->Measure - ldob->Last_Measure) / ldob->dt;

    // 计算测量信号的二阶导数（加速度）
    if (ldob->Measure_ddot_OLS_Order > 2)
        ldob->Measure_ddot = OLS_Derivative(&ldob->Measure_ddot_OLS, ldob->dt, ldob->Measure_dot);
    else
        ldob->Measure_ddot = (ldob->Measure_dot - ldob->Last_Measure_dot) / ldob->dt;

    // 计算干扰估计值
    ldob->Disturbance = ldob->c[0] * ldob->Measure + ldob->c[1] * ldob->Measure_dot + ldob->c[2] * ldob->Measure_ddot - ldob->u;

    // 对干扰估计值进行低通滤波
    ldob->Disturbance = ldob->Disturbance * ldob->dt / (ldob->LPF_RC + ldob->dt) + ldob->Last_Disturbance * ldob->LPF_RC / (ldob->LPF_RC + ldob->dt);

    // 限制干扰估计值在最大干扰范围内
    ldob->Disturbance = fp32_constrain(ldob->Disturbance, -ldob->Max_Disturbance, ldob->Max_Disturbance);

    // 如果干扰估计超过死区，则输出干扰值，否则输出 0
    if (abs(ldob->Disturbance) > ldob->DeadBand * ldob->Max_Disturbance)
        ldob->Output = ldob->Disturbance;
    else
        ldob->Output = 0;

    // 保存当前测量值和干扰估计值，用于下一次计算
    ldob->Last_Measure = ldob->Measure;
    ldob->Last_Measure_dot = ldob->Measure_dot;
    ldob->Last_Disturbance = ldob->Disturbance;

    return ldob->Output;
}

/*************************** Tracking Differentiator ***************************/

/**
  * @brief          初始化TD控制器（Thermal Diffusion Controller）的参数
  * @author         冉文治
  * @param          td      指向 TD 控制器结构体的指针，用于存储初始化后的控制器状态
  * @param          r       控制器的系数 r，用于调节热扩散的控制力度
  * @param          h0      初始的热扩散值 h0，用于控制器的初始化
  * @retval         无返回值。初始化后的 TD 控制器将保存在 `td` 结构体中。
  *
  * @note           此函数用于初始化 TD 控制器的各项参数，包括控制系数 `r` 和初始值 `h0`，
  *                 并重置状态变量（`x`, `dx`, `ddx`等）。初始化时，`x`, `dx`, `ddx`等状态
  *                 变量被设为零，以便控制器在后续计算中使用。
  */
void TD_Init(TD_t *td, fp32 r, fp32 h0)
{
    // 初始化控制系数
    td->r = r;
    td->h0 = h0;

    // 重置状态变量
    td->x = 0;
    td->dx = 0;
    td->ddx = 0;
    td->last_dx = 0;
    td->last_ddx = 0;
}

/**
  * @brief          计算并更新 TD 控制器的输出
  * @author         冉文治
  * @param          td      指向 TD 控制器结构体的指针，包含当前的控制器状态和参数
  * @param          input   输入信号，用于控制器计算
  * @retval         控制器的输出值
  *
  * @note           此函数根据输入信号 `input` 和控制器的参数计算新的状态值，并返回控制器的输出。
  *                 控制器使用热扩散算法进行计算。首先，根据输入信号更新 `x`, `dx`, `ddx` 等状态变量，
  *                 然后返回新的状态值 `x` 作为控制输出。
  */
fp32 TD_Calculate(TD_t *td, fp32 input)
{
    // 临时变量声明
    static fp32 d, a0, y, a1, a2, a, fhan;

    // 获取时间增量
    td->dt = DWT_GetDeltaT((void *)&td->DWT_CNT);

    // 如果时间增量过大，则不进行计算
    if (td->dt > 0.5f)
        return 0;

    // 更新输入信号
    td->Input = input;

    // 计算中间变量
    d = td->r * td->h0 * td->h0;
    a0 = td->dx * td->h0;
    y = td->x - td->Input + a0;
    a1 = sqrt(d * (d + 8 * abs(y)));
    a2 = a0 + sign(y) * (a1 - d) / 2;
    a = (a0 + y) * (sign(y + d) - sign(y - d)) / 2 + a2 * (1 - (sign(y + d) - sign(y - d)) / 2);
    fhan = -td->r * a / d * (sign(a + d) - sign(a - d)) / 2 - td->r * sign(a) * (1 - (sign(a + d) - sign(a - d)) / 2);

    // 更新状态变量：加速度、速度和位置
    td->ddx = fhan;
    td->dx += (td->ddx + td->last_ddx) * td->dt / 2;
    td->x += (td->dx + td->last_dx) * td->dt / 2;

    // 保存上次的加速度和速度，供下次使用
    td->last_ddx = td->ddx;
    td->last_dx = td->dx;

    // 返回控制器的输出（位置）
    return td->x;
}
